/*******************************************************************************
 *
 *      Filename: Matrix3x3.cpp
 *   Description: See Matrix3x3.h
 *        Author: [kb]ypp
 *    Created at: October 24, 2005
 * Last modified: 
 *
 ******************************************************************************/

#include "Matrix3x3.h"
#include "MathUtil.h"
#include "Vector3.h"
#include "../core/Log.h"
#include "cstring" // for memcpy()

//! Does not perform any initialization
//!
mat3::mat3()
{}

mat3::mat3( const mat3 &mat )
	: m11( mat.m11 ), m21( mat.m21 ), m31( mat.m31 ),
	  m12( mat.m12 ), m22( mat.m22 ), m32( mat.m32 ),
	  m13( mat.m13 ), m23( mat.m23 ), m33( mat.m33 )
{}

mat3::mat3( float a11, float a12, float a13,
				float a21, float a22, float a23,
				float a31, float a32, float a33 )
	: m11( a11 ), m21( a21 ), m31( a31 ),
	  m12( a12 ), m22( a22 ), m32( a32 ),
	  m13( a13 ), m23( a23 ), m33( a33 )
{}

const mat3& mat3::operator=( const mat3& rv )
{
	if ( this != &rv ) // Self-assignment check
		::memcpy( m, rv.m, sizeof m );
	return *this;
}

//! @f[ \left( \begin{array}{ccc}
//!		m_{11} & m_{12} & m_{13} \\
//!		m_{21} & m_{22} & m_{23} \\
//!		m_{31} & m_{32} & m_{33} \\
//!	\end{array} \right) @f]
//!
const mat3 operator*( const mat3 &a, const mat3 &b )
{
	return mat3(
		a.m11 * b.m11 + a.m12 * b.m21 + a.m13 * b.m31, // m11
		a.m11 * b.m12 + a.m12 * b.m22 + a.m13 * b.m32, // m12
		a.m11 * b.m13 + a.m12 * b.m23 + a.m13 * b.m33, // m13
		a.m21 * b.m11 + a.m22 * b.m21 + a.m23 * b.m31, // m21
		a.m21 * b.m12 + a.m22 * b.m22 + a.m23 * b.m32, // m22
		a.m21 * b.m13 + a.m22 * b.m23 + a.m23 * b.m33, // m23
		a.m31 * b.m11 + a.m32 * b.m21 + a.m33 * b.m31, // m31
		a.m31 * b.m12 + a.m32 * b.m22 + a.m33 * b.m32, // m32
		a.m31 * b.m13 + a.m32 * b.m23 + a.m33 * b.m33  // m33
	);
}

const vec3 operator*( const mat3 &mat, const vec3 &v )
{
	return vec3(
		/* x */ mat.m11 * v.x + mat.m12 * v.y + mat.m13 * v.z,
		/* y */ mat.m21 * v.x + mat.m22 * v.y + mat.m23 * v.z,
		/* z */ mat.m31 * v.x + mat.m32 * v.y + mat.m33 * v.z
	);
}

//! Not very fast.
//!
bool operator==( const mat3 &lv, const mat3 &rv )
{
	return ( fabs( lv.m11 - rv.m11 ) < EPSILON &&
			 fabs( lv.m12 - rv.m12 ) < EPSILON &&
			 fabs( lv.m13 - rv.m13 ) < EPSILON &&
			 fabs( lv.m21 - rv.m21 ) < EPSILON &&
			 fabs( lv.m22 - rv.m22 ) < EPSILON &&
			 fabs( lv.m23 - rv.m23 ) < EPSILON &&
			 fabs( lv.m31 - rv.m31 ) < EPSILON &&
			 fabs( lv.m32 - rv.m32 ) < EPSILON &&
			 fabs( lv.m33 - rv.m33 ) < EPSILON );
}

//! Not very fast.
//!
bool operator!=( const mat3 &lv, const mat3 &rv )
{
	return !( lv == rv );
}

void mat3::loadIdentity()
{
	m11 = 1.0;  m12 = 0.0;  m13 = 0.0;
	m21 = 0.0;  m22 = 1.0;  m23 = 0.0;
	m31 = 0.0;  m32 = 0.0;  m33 = 1.0;
}

float det( const mat3 &mat )
{
	return mat.m11 * ( mat.m22 * mat.m33 - mat.m23 * mat.m32 ) -
		   mat.m12 * ( mat.m21 * mat.m33 - mat.m23 * mat.m31 ) +
		   mat.m13 * ( mat.m21 * mat.m32 - mat.m22 * mat.m31 );
}

const mat3 transpose( const mat3 &mat )
{
	return mat3( mat.m11, mat.m21, mat.m31,
				 mat.m12, mat.m22, mat.m32,
				 mat.m13, mat.m23, mat.m33 );
}

//! If the matrix `mat' is singular than the result is undefined
//! (also warning message will be sent to system log).
//!
const mat3 inv( const mat3 &mat )
{
	float d = det( mat );
	if ( fabs( d ) < EPSILON ) { // Check if the matrix is singular
		LOG_WARNING( "The matrix is singular. Its inverse can't be found" );
		return mat3();
	}
	
	float invDet = 1.0 / d;
	return mat3(
		/* m11 */  invDet * ( mat.m22 * mat.m33 - mat.m23 * mat.m32 ),
		/* m12 */ -invDet * ( mat.m12 * mat.m33 - mat.m13 * mat.m32 ),
		/* m13 */  invDet * ( mat.m12 * mat.m23 - mat.m13 * mat.m22 ),
		/* m21 */ -invDet * ( mat.m21 * mat.m33 - mat.m23 * mat.m31 ),
		/* m22 */  invDet * ( mat.m11 * mat.m33 - mat.m13 * mat.m31 ),
		/* m23 */ -invDet * ( mat.m11 * mat.m23 - mat.m13 * mat.m21 ),
		/* m31 */  invDet * ( mat.m21 * mat.m32 - mat.m22 * mat.m31 ),
		/* m32 */ -invDet * ( mat.m11 * mat.m32 - mat.m12 * mat.m31 ),
		/* m33 */  invDet * ( mat.m11 * mat.m22 - mat.m12 * mat.m21 )
	);
}

//! Counter-clockwise rotation angle `theta' given in radians.
//!
const mat3 mat3::rotateX( float theta )
{
	const float c = cos( theta );
	const float s = sin( theta );
	return mat3(
		1.0, 0.0, 0.0,
		0.0,   c,  -s,
		0.0,   s,   c
	);
}

//! Counter-clockwise rotation angle `theta' given in radians.
//!
const mat3 mat3::rotateY( float theta )
{
	const float c = cos( theta );
	const float s = sin( theta );
	return mat3(
		  c, 0.0,   s,
		0.0, 1.0, 0.0,
		 -s, 0.0,   c
	);
}

//! Counter-clockwise rotation angle `theta' given in radians.
//!
const mat3 mat3::rotateZ( float theta )
{
	const float c = cos( theta );
	const float s = sin( theta );
	return mat3(
		  c,  -s, 0.0,
		  s,   c,  0.0,
		0.0, 0.0, 1.0
	);
}

//! Rotation axis `axis' assumed to be a unit vector.
//! Counter-clockwise rotation angle `theta' given in radians.
//!
const mat3 mat3::rotateAxis( const vec3 &axis, float theta )
{
	const float c = cos( theta );
	const float one_c = 1.0 - c; // ( 1 - cos )
	const float s = sin( theta );
	
	const float xx = axis.x * axis.x;
	const float yy = axis.y * axis.y;
	const float zz = axis.z * axis.z;
	const float xy = axis.x * axis.y;
	const float yz = axis.y * axis.z;
	const float xz = axis.x * axis.z;
	const float xsin = axis.x * s;
	const float ysin = axis.y * s;
	const float zsin = axis.z * s;
	
	return mat3 (
		   c + one_c * xx, one_c * xy - zsin, one_c * xz + ysin,
		one_c * xy + zsin,    c + one_c * yy, one_c * yz - xsin,
		one_c * xz - ysin, one_c * yz + xsin,    c + one_c * zz );
}
